#include <stdio.h>
#include <math.h>
#include "delta_x.h"

//因为这里只需要对一个机械臂进行解算，所以不需要传入指针解算
//直接对全局变量进行计算反而更加方便
delta delta_x;

void delta_init(void) {

	delta_x.R = R_b;
	delta_x.r = R_f;
	delta_x.L = L_b;
	delta_x.l = L_f;

}

void delta_calcForward(float theta1, float theta2, float theta3)
{
//1.
float A1=0;
float A2=0;
float A3=0;
float B1=0;
float B2=0;
float B3=0;
float C1=0;
float C2=0;
float C3=0;

A1 =  (delta_x.R + delta_x.L * cosf(theta1) - delta_x.r);
A2 = -(delta_x.R + delta_x.L * cosf(theta2) - delta_x.r)/2;
A3 = -(delta_x.R + delta_x.L * cosf(theta3) - delta_x.r)/2;

B1 = 1;
B2 = (sqrt(3) * (delta_x.R + delta_x.L * cosf(theta2) - delta_x.r))/2;
B3 = -(sqrt(3) * (delta_x.R + delta_x.L * cosf(theta3) - delta_x.r))/2;
	
C1 = delta_x.L * sinf(theta1);
C2 = delta_x.L * sinf(theta2);
C3 = delta_x.L * sinf(theta3);

//2.
float D1=0;
float A21=0;
float C21=0;
float D2=0;
float A31=0;
float C31=0;
D1 = (A1*A1 - A2*A2 + C1*C1 - C2*C2 - B2*B2)/2;
D2 = (A1*A1 - A3*A3 + C1*C1 - C3*C3 - B3*B3)/2;
A21 = A2 - A1;
A31 = A3 - A1;
C21 = C2 - C1;
C31 = C3 - C1;

//3.
float E1=0;
float F1=0;
float E2=0;
float F2=0;
E1 = (B3 * C21 - B2 * C31)/(A21 * B3 - A31 * B2);
F1 = (B2 * D2  - B3 * D1 )/(A21 * B3 - A31 * B2);
E2 = (A31* C21 - A21* C31)/(A31 * B2 - A21 * B3);
F2 = (A21* D2  - A31* D1 )/(A31 * B2 - A21 * B3);


//4.
float a=0;
float b=0;
float c=0;
a = E1 * E1 + E2 * E2 + 1;

b = 2*E2*F2 +2*C1 - 2*E1*(A1-F1);

c = (A1-F1)*(A1-F1) + F2*F2 + C1*C1 - delta_x.l * delta_x.l;
//5.
delta_x.z_p = (-b-(sqrt(b*b - 4*a*c)))/(2*a);
delta_x.x_p = E1*delta_x.z_p + F1;
delta_x.y_p = E2*delta_x.z_p + F2;
}

int main(){

	delta_init();
	delta_calcForward(10.3719,10.3719,10.3719);
	printf("%lf",delta_x.x_p);
	printf("%lf",delta_x.y_p);
	printf("%lf",delta_x.z_p);
	return 0;

}












